package it.gius.processing.gjkTest;


import org.jbox2d.common.Vec2;

import processing.core.PApplet;
import it.gius.pe.Shape.Polygon;
import it.gius.pe.algorithm.gjk.DistanceSolution;
import it.gius.pe.algorithm.gjk.GjkEpaAlgorithm;
import it.gius.pe.algorithm.gjk.NotConvergingException;
import it.gius.pe.algorithm.gjk.shapes.GjkPolygon;
import it.gius.pe.mains.MainMovingTest;
import it.gius.processing.util.MyAbstractPApplet;

public class GjkPolyPointMotionApplet extends MyAbstractPApplet {

	private Polygon poly = null;
	private Vec2 q = null;

	public GjkPolyPointMotionApplet(Polygon poly,Vec2 q) {
		this.poly = poly;
		this.q = q;
	}


	public void setup(){

		size(MainMovingTest.sizeX,MainMovingTest.sizeY);
		smooth();

	}


	public void draw()
	{

		if(!goStop.isGo())
			noLoop();

		background(220);


		poly.move();
		moveQ(q);
		

		DistanceSolution sol = new DistanceSolution();
		try {
			GjkEpaAlgorithm.distance(new GjkPolygon(poly), q, sol);
			//GjkAlgorithm.distance(poly, q, sol);
			if(sol.penetration)
			{
				System.out.println("penetrationDepth: " + sol.penetrationDepth);
				System.out.println("normalPenetration: "+sol.normalPenetration);
				line(q.x,q.y ,q.x+30*sol.normalPenetration.x,q.y+30*sol.normalPenetration.y);
			}
			//println("distance: " + sol.distance);
		} catch (NotConvergingException e) {
			e.printStackTrace();
			println("gjk algorith is not converging");
			println("sol.distance: " + sol.distance);
			println("sol.p1: "+sol.p1);
			goStop.setGo(false);
		}


		fill(0,0,210);
		stroke(10);

		if(sol.distance == 0)
			fill(210,0,0);
		
		drawPoly(poly);



		fill(210,0,0);
		ellipse(q.x,q.y,3,3);
		
		
		stroke(210,210,0);
		line(sol.p1.x,sol.p1.y,q.x,q.y);


	}
	
	private void moveQ(Vec2 q)
	{
		q.x = q.x -0.5f;
		q.y = q.y - 0.1f;
	}

	private void drawPoly(Polygon poly)
	{
		beginShape();
		for(int i=0; i< poly.getVerticesCount(); i++)
		{
			Vec2 v = poly.getVertex(i);
			vertex(v.x,v.y);
		}
		endShape(PApplet.CLOSE);

	}

}
